#include "node/starlight_camera.hpp"
#include <cv_bridge/cv_bridge.h>

using namespace WD;
using json = nlohmann::json;

StarlightCamera::StarlightCamera(ros::NodeHandle& nh, ros::NodeHandle& nh_private)
    : nh_(nh), nh_private_(nh_private),it_(nh)
{ 
}

StarlightCamera::~StarlightCamera()
{
    running_ = false;

    std::cout << "[StarlightCamera] Closing camera..." << std::endl;
    control_sdk_ptr_->closeCamera(true);
    std::cout << "[StarlightCamera] Camera closed" << std::endl;

    std::cout << "[StarlightCamera] Stopping decoder..." << std::endl;
    decoder_ptr_->stop();
    std::cout << "[StarlightCamera] Decoder stopped" << std::endl;

    image_split_queue_.notifyAll();  // ✅ 唤醒阻塞的线程
    if (image_split_pub_thread_.joinable()) {
        image_split_pub_thread_.join();  // 或 detach()
    }
}

void WD::StarlightCamera::runStream()
{
    // 多路流（camera_id = 1~4）
    registerStreamCallback(1, pub_image_1_);
    registerStreamCallback(2, pub_image_2_);
    registerStreamCallback(3, pub_image_3_);
    registerStreamCallback(4, pub_image_4_);

    decoder_ptr_->setStitchedFrameCallback([this](const FrameWithTimestamp& msg) {
        publishFrame(msg, pub_image_split_);
    });

    try {
        decoder_ptr_->start();
    } catch (const std::exception& e) {
        ROS_FATAL("Exception caught in decoder start: %s", e.what());
        return;
    }

    ROS_INFO("TCP连接成功，开始接收图像数据...");
}

// void StarlightCamera::publishFrame(const FrameWithTimestamp& msg, const image_transport::Publisher& pub)
// {
//     if (msg.timestamp < 0) {
//         ROS_WARN("收到非法负时间戳: %f，跳过此帧", msg.timestamp);
//         return;  // 或 continue;
//     }
//     sensor_msgs::ImagePtr pub_msg =
//         cv_bridge::CvImage(std_msgs::Header(), "bgr8", msg.image).toImageMsg();
//     pub_msg->header.stamp = ros::Time(msg.timestamp);
//     pub_msg->header.frame_id = "camera_" + std::to_string(msg.camera_id);
//     if (pub == pub_image_split_) {
//         image_split_queue_.push(pub_msg);  // 🎯 推入发布队列
//     } else {
//         pub.publish(pub_msg);  // 其余分路图像仍原地发布
//     }
// }

void StarlightCamera::publishFrame(const FrameWithTimestamp& msg, const image_transport::Publisher& pub)
{
    if (msg.timestamp < 0) {
        ROS_WARN("收到非法负时间戳: %f，跳过此帧", msg.timestamp);
        return;
    }

    // 初始化输出图像
    cv::Mat rotated_img;

    if (!stitching_) {
        // stitching_ 为 false 时，按 camera_id 进行图像旋转
        switch (msg.camera_id) {
            case 1:
                cv::rotate(msg.image, rotated_img, cv::ROTATE_90_COUNTERCLOCKWISE);
                break;
            case 4:
                cv::rotate(msg.image, rotated_img, cv::ROTATE_90_CLOCKWISE);
                break;
            case 2:
                cv::rotate(msg.image, rotated_img, cv::ROTATE_90_CLOCKWISE);
                break;
            case 3:
                cv::rotate(msg.image, rotated_img, cv::ROTATE_90_COUNTERCLOCKWISE);
                break;
            default:
                rotated_img = msg.image.clone();  // 不旋转
                break;
        }
    } else {
        // stitching_ 为 true，直接使用原图像
        rotated_img = msg.image.clone();
    }

    sensor_msgs::ImagePtr pub_msg =
        cv_bridge::CvImage(std_msgs::Header(), "bgr8", rotated_img).toImageMsg();

    pub_msg->header.stamp = ros::Time(msg.timestamp);
    pub_msg->header.frame_id = "camera_" + std::to_string(msg.camera_id);

    if (pub == pub_image_split_) {
        image_split_queue_.push(pub_msg);  // 推入发布队列
    } else {
        pub.publish(pub_msg);  // 分路图像原地发布
    }
}


void StarlightCamera::registerStreamCallback(int camera_id, const image_transport::Publisher& pub)
{
    decoder_ptr_->setStreamCallback(camera_id, [this, pub](const FrameWithTimestamp& msg) {
        publishFrame(msg, pub);
    });
}

void StarlightCamera::initCamera()
{
    control_sdk_ptr_ = std::make_unique<WD::ControlSdk>(); 
    // auto camera_info = control_sdk_ptr_->queryCamera(); // 查询相机
    
    if (!control_sdk_ptr_->connectCamera(this->ip_params_.ip)) {
        ROS_ERROR("未找到相机，请检查网络连接或相机状态！%s",this->ip_params_.ip.c_str());
        
        return;
    }
    control_sdk_ptr_->stopStream();
    ROS_INFO("连接到相机: %s", this->ip_params_.ip.c_str());

    // if (!control_sdk_ptr_->setCameraIpParams(this->ip_params_)) {
    //     ROS_ERROR("设置相机 IP 参数失败！");
    //     return;
    // }
    // 设置图像输出参数
    if (!control_sdk_ptr_->setImageOutParams(this->image_params_)) {
        ROS_ERROR("设置图像输出参数失败！");
        return;
    }

    // 设置 GPS 参数
    if (!control_sdk_ptr_->setGpsParams(this->gps_params_)) {
        ROS_ERROR("设置 GPS 参数失败！");
        return;
    }

    // 设置 PPS 参数
    if (!control_sdk_ptr_->setPpsParams(this->pps_params_)) {
        ROS_ERROR("设置 PPS 参数失败！");
        return;
    }

    // 开关 GPS
    if (!control_sdk_ptr_->setGpsEnable(true)) {
        ROS_ERROR("启用 GPS 失败！");
        return;
    }

    // 开关 PPS
    if (!control_sdk_ptr_->setPpsEnable(true)) {
        ROS_ERROR("启用 PPS 失败！");
        return;
    }
    
    
    // 开始推流
    control_sdk_ptr_->startStream();
    ROS_INFO("相机初始化完成，开始推流...");
    int max_streams = image_params_.stitching ? 1 : 4;
    decoder_ptr_ = std::make_unique<WD::FrameDecoder>("tcp://" + this->ip_params_.ip + ":9000", max_streams);
    //ROS_INFO("tcpip...: %s", this->ip_params_.ip.c_str());
}

void WD::StarlightCamera::initRos()
{
    // YAML → JSON
    json j = yamlToJson();
    // 转换为结构体
    image_params_ = j.get<ImageOutParams>();
    pps_params_  = j.get<PpsConfigParams>();
    gps_params_ = j.get<GpsParams>();
    ip_params_ = j.get<IpConfigParams>();

    std::string send_images_topic_1 = 
        nh_private_.param<std::string>("ros/send_images_topic_1", "/starlight/image_1");
    std::string send_images_topic_2 = 
        nh_private_.param<std::string>("ros/send_images_topic_2", "/starlight/image_2");
    std::string send_images_topic_3 = 
        nh_private_.param<std::string>("ros/send_images_topic_3", "/starlight/image_3");
    std::string send_images_topic_4 = 
        nh_private_.param<std::string>("ros/send_images_topic_4", "/starlight/image_4");

    std::string send_images_split_topic = 
        nh_private_.param<std::string>("ros/send_images_split_topic", "/starlight/image_split");



    // ✅ 创建发布者
    pub_image_1_ = it_.advertise(send_images_topic_1, 1);
    pub_image_2_ = it_.advertise(send_images_topic_2, 1);
    pub_image_3_ = it_.advertise(send_images_topic_3, 1);
    pub_image_4_ = it_.advertise(send_images_topic_4, 1);
    pub_image_split_ = it_.advertise(send_images_split_topic, 1);

    image_split_pub_thread_ = std::thread([this]() {
        ros::Rate rate(100);  // 控制频率，防止空转
        while (ros::ok()) {
            sensor_msgs::ImagePtr msg;
            if (image_split_queue_.pop(msg,running_)) {
                pub_image_split_.publish(msg);
            }
            rate.sleep();
        }
    });
}

json StarlightCamera::yamlToJson()
{
    json j;

    // 图像输出参数
    j["type"]         = nh_private_.param<std::string>("/image/type", "");
    j["enable_image_capture_notify"] = nh_private_.param<bool>("/image/enable_image_capture_notify", false);
    j["w"]            = nh_private_.param<int>("/image/width", 3840);
    j["h"]            = nh_private_.param<int>("/image/height", 2160);
    j["fps"]          = nh_private_.param<int>("/image/fps", 5);
    stitching_        = nh_private_.param<bool>("/image/stitching", true);
    j["stitching"]    = nh_private_.param<bool>("/image/stitching", false);
    j["num"]          = nh_private_.param<int>("/image/num", 0);
    j["burst"]        = nh_private_.param<int>("/image/burst", 1);
    j["template"]     = nh_private_.param<std::string>("/image/template", "");
    j["stabilization"]= nh_private_.param<bool>("/image/stabilization", true);
    j["overlay_image_name"] = nh_private_.param<std::string>("/image/overlay_image_name", "TECHE3.png");
    j["overlay_location"]   = nh_private_.param<int>("/image/overlay_location", 0);
    j["overlay_range"]      = nh_private_.param<int>("/image/overlay_range", 5);

    // PPS 参数
    j["enable_notify"] = nh_private_.param<bool>("/pps/enable_notify", true);
    j["interval"]      = nh_private_.param<int>("/pps/interval", 1000000);
    j["tolerance"]     = nh_private_.param<int>("/pps/tolerance", 1000);
    j["verify"]        = nh_private_.param<int>("/pps/verify", 10);
    j["polarity"]      = nh_private_.param<int>("/pps/polarity", 1);

    // GPS 参数
    j["enable_notify"] = nh_private_.param<bool>("/gps/enable_notify", true);
    j["baudrate"]      = nh_private_.param<int>("/gps/baudrate", 115200);
    j["sync_mode"]     = nh_private_.param<int>("/gps/sync_mode", 0);

    // IP 配置参数
    j["ip"]       = nh_private_.param<std::string>("/camera/ip", "192.168.1.100");
    j["mask"]       = nh_private_.param<std::string>("/camera/mask", "255.255.255.0");
    j["gateway"]       = nh_private_.param<std::string>("/camera/gateway", "192.168.1.1");
    j["dns"]       = nh_private_.param<std::string>("/camera/dns", "8.8.8.8");

    return j;
}
